#include "StdAfx.h"
#include "BresehamLine3D.h"
#include "SlowMotion.h"
#include "Point3D.h"
#include <math.h>
#include <algorithm>
#include <assert.h>
#include <ppl.h>
#include <vector>
using namespace std;

#define SWAP(a,b){MY_DATATYPE c; c = a;a=b;b=c;}

BresehamLine3D::BresehamLine3D(ConfigurationState * _previous,ConfigurationState * _current):LineInterpolation(_previous,_current)
{
}

BresehamLine3D::~BresehamLine3D(void)
{
}

void BresehamLine3D::clamp(UINT machineWidth,UINT machineHeight,vector<ConfigurationState*> *& states)
{
	SlowMotion * prevSM = dynamic_cast<SlowMotion*>(this->previous);
	SlowMotion * currSM = dynamic_cast<SlowMotion*>(this->current);
	if(abs(prevSM->x - currSM->x)<EPSILON)
	{
		if(prevSM->y< (MY_DATATYPE) 0.0)
			prevSM->y = (MY_DATATYPE) 0.0;
		if(prevSM->y>= (MY_DATATYPE) machineHeight)
			prevSM->y = (MY_DATATYPE) ( machineHeight-1);

		if(currSM->y< (MY_DATATYPE) 0.0)
			currSM->y = (MY_DATATYPE) 0.0;
		if(currSM->y>= (MY_DATATYPE) machineHeight)
			currSM->y = (MY_DATATYPE) (machineHeight-1);
	}
	else if(abs(prevSM->y - currSM->y)<EPSILON)
	{ 
		if(prevSM->x < (MY_DATATYPE) 0.0)
			prevSM->x = (MY_DATATYPE) 0.0;
		if(prevSM->x >= (MY_DATATYPE) machineWidth)
			prevSM->x = (MY_DATATYPE) (machineWidth - 1);
		if(currSM->x < (MY_DATATYPE) 0.0)
			prevSM->x = (MY_DATATYPE) (machineWidth - 1);
		if(currSM->x >= (MY_DATATYPE) machineWidth)
			currSM->x = (MY_DATATYPE) (machineWidth -1);
	}
	else{
		MY_DATATYPE a = (currSM->y - prevSM->y)/(currSM->x - prevSM->x);
		MY_DATATYPE deltaZ = (currSM->z - prevSM->z)/(currSM->x - prevSM->x);
		if(prevSM->x < (MY_DATATYPE) 0.0)
		{
			prevSM->y += (-prevSM->x)*a;
			prevSM->z += (-prevSM->z)*deltaZ;
			prevSM->x = (MY_DATATYPE) 0.0;
		}
		if(prevSM->x>= (MY_DATATYPE)machineWidth)
		{
			prevSM->y += (prevSM->x - machineWidth)*a;
			prevSM->z= (prevSM->x - machineWidth)*deltaZ;
			prevSM->x = (MY_DATATYPE) (machineWidth-1);
		}
		if(currSM->x < (MY_DATATYPE) 0.0)
		{
			currSM->y += (-currSM->x)*a;
			currSM->z += (-currSM->z)*deltaZ;
			currSM->x = (MY_DATATYPE) 0.0;
		}
		if(currSM->x>= (MY_DATATYPE) machineWidth)
		{
			currSM->y += (currSM->x - machineWidth)*a;
			currSM->z= (currSM->x - machineWidth)*deltaZ;
			currSM->x = (MY_DATATYPE) (machineWidth-1);
		}
	}
}

void BresehamLine3D::updateNeighbourhood(UINT machineWidth,UINT machineHeight,bool steep,MY_DATATYPE x,MY_DATATYPE y,MY_DATATYPE z, MY_DATATYPE stepX,MY_DATATYPE stepY,MY_DATATYPE stepZ,int & instructionNumber,vector<Point3D> *& neighbourhood, vector<ConfigurationState*> *& states)
{
	if(steep)
	{
		Concurrency::parallel_for_each(neighbourhood->begin(),neighbourhood->end(),[&](Point3D & p)
		{
			if((p.y+stepY)<machineHeight && (p.y+stepY)>=0)
			{
				p.y += stepY;
				p.z += stepZ;
			}
		});
		SlowMotion * m = new SlowMotion(instructionNumber++,(MY_DATATYPE) y,(MY_DATATYPE) x, (MY_DATATYPE) z);
		states->push_back(m);
	}
	else
	{
		Concurrency::parallel_for_each(neighbourhood->begin(),neighbourhood->end(),[&](Point3D & p)
		{
			if((p.x+stepX)<machineWidth && (p.x+stepX)>=0)
			{
				p.x += stepX;
				p.z += stepZ;
			}
		});
		SlowMotion * m = new SlowMotion(instructionNumber++,(MY_DATATYPE) x, (MY_DATATYPE) y,(MY_DATATYPE) z);

		states->push_back(m);
	}
}

void BresehamLine3D::checkMilliingError(MillingMachine3C * machine,MillingTool * tool,int & instructionNumber,ErrorInfo ** millingErrorData,vector<Point3D> *& neighbourhood, vector<ConfigurationState*> *& states)
{
	for_each(neighbourhood->begin(),neighbourhood->end(),[states,tool,machine,&instructionNumber,&millingErrorData](Point3D p)
	{
		if(p.x<0.0 || p.x>=machine->width)
			return;
		if(p.y<0.0 || p.y>=machine->height)
			return;

		double ex = p.x - tool->posX;
		double ey = p.y - tool->posY;
		double val = (tool->diameter/2)*(tool->diameter/2) - ex*ex - ey*ey;
		if(val<0.0)
			return;

		double err =  tool->GetMillingErrorAt(p);
		int indX = (int)(p.x);
		int indY = (int)(p.y);

		assert(indX>=0);
		assert(indY>=0);

		assert((UINT)indX <= machine->width);
		assert((UINT)indY <= machine->height);

		if(millingErrorData[indX][indY].error>err)
		{
			if(millingErrorData[indX][indY].state!=nullptr)
			{
				remove(states->begin(),states->end(),millingErrorData[indX][indY].state);
			}
			SlowMotion * m = new SlowMotion(instructionNumber++,p.x,p.y,p.z + (MY_DATATYPE)err);
			millingErrorData[indX][indY].error = err;
			millingErrorData[indX][indY].state = m;
			states->push_back(m);
		}
	});
}

void BresehamLine3D::perform(MillingMachine3C * machine,MillingTool * tool,vector<ConfigurationState*> *& states,ErrorInfo ** millingErrorData)
{	
	clamp(machine->width,machine->height,states);
	SlowMotion * prevSM = dynamic_cast<SlowMotion*>(this->previous);
	prevSM->isKeyState = true;

	SlowMotion * currSM = dynamic_cast<SlowMotion*>(this->current);
	currSM->isKeyState = true;
	states->pop_back();

	tool->adjustDirection(Point3D(prevSM->x,prevSM->y,prevSM->z),Point3D(currSM->x,currSM->y,currSM->z));

	int lastInstructionNumber = prevSM->number;
	MY_DATATYPE x0, x1, delta_x;
	MY_DATATYPE y, y0, y1, delta_y, step_y;
	MY_DATATYPE z, z0, z1, step_z;    

	//start and end points
	x0 = prevSM->x;     x1 = currSM->x;
	y0 = prevSM->y;     y1 = currSM->y;
	z0 = prevSM->z;     z1 = currSM->z;

	bool steep = abs(y1-y0)>abs(x1-x0);
	if(steep)
	{
		SWAP(x0,y0);
		SWAP(x1,y1);
	}
	delta_x = abs(x1-x0);
	delta_y =  y1-y0;
	MY_DATATYPE error = delta_x/2;

	y = y0;
	MY_DATATYPE inc = (MY_DATATYPE) 1.0;
	if(x0<=x1)
		inc=1;
	else
		inc = -1;

	if(y0<=y1)
		step_y = 1;
	else 
		step_y = -1;

	if(delta_x!=0.0)
		step_z =  (z1-z0)/delta_x;
	else if(delta_y!=0.0)
		step_z = (z1-z0)/abs(delta_y);

	z = z0;

	double tx = tool->posX;
	double ty = tool->posY;
	double tz = tool->posZ;

	if(steep)
	{
		tool->posX = y;
		tool->posY = x0;
		tool->posZ = z;
	}
	else
	{
		tool->posX = x0;
		tool->posY = y;
		tool->posZ = z;
	}

	vector<Point3D> * neighbourhood = tool->GetToolSurfacePoints();
	UINT machineWidth = machine->width;
	UINT machineHeight = machine->height;
	for(MY_DATATYPE x = x0; inc>0 ? x<x1 : x>x1 ; x+=inc)
	{
		this->checkMilliingError(machine,tool,lastInstructionNumber,millingErrorData,neighbourhood,states);

		double change = delta_y==0.0 ? delta_y : delta_y/(x1-x0)*inc;
		if(steep)
		{

			this->updateNeighbourhood(machineWidth,machineHeight,steep,(MY_DATATYPE) x, (MY_DATATYPE) y,(MY_DATATYPE) z,(MY_DATATYPE)change,inc,step_z,lastInstructionNumber,neighbourhood,states);
			tool->posX += change;
			tool->posY += inc;
			tool->posZ += step_z;
		}
		else
		{

			this->updateNeighbourhood(machineWidth,machineHeight,steep,(MY_DATATYPE) x, (MY_DATATYPE) y,(MY_DATATYPE) z,inc,(MY_DATATYPE)change,step_z,lastInstructionNumber,neighbourhood,states);
			tool->posX += inc;
			tool->posY += change;
			tool->posZ += step_z;
		}
	}
	tool->posX = tx;
	tool->posY = ty;
	tool->posZ = tz;
	neighbourhood->clear();
	delete neighbourhood;
	states->push_back(currSM);
}
